#include "faceprocessor.h"
#include "QString"

FaceProcessor::FaceProcessor()
{
    detector_modelpath = QString("/home/fz/wj-work/face/EasyFace-main/models/retinaface/").toStdString();
    recognizer_modelpath = QString("/home/fz/wj-work/face/EasyFace-main/models/mobilefacenet/").toStdString();;
    register_imagepath = QString("/home/fz/wj-work/face/EasyFace-main/images/register/").toStdString();;
    identify_filename = QString("/home/fz/wj-work/face/EasyFace-main/images/identify/img1.jpg").toStdString();;



    detector =new easyface::FaceDetector(detector_modelpath);
    recognizer = new easyface::FaceRecognizer(recognizer_modelpath);
    username_num_map.clear();
}

FaceProcessor::~FaceProcessor(){

    if(detector!= NULL)
        delete detector;
    if(recognizer!= NULL)
        delete recognizer;
}


void FaceProcessor::face_register(easyface::FaceDetector *detector,
                                  easyface::FaceRecognizer *recognizer,
                                const std::string &register_imagepath,
                                  std::map<std::string,
                                  std::vector<float>> &username_feature_map
                                  )
{
    std::string pattern;
    if(register_imagepath.back() == '/' || register_imagepath.back() == '\\')
    {
        pattern = register_imagepath + "*.*";
    }
    else
    {
        pattern = register_imagepath + "/" + "*.*";
    }

    std::vector<cv::String> register_filenames;
    cv::glob(pattern, register_filenames);

    std::cout << "Start face register... " << std::endl;
    for(auto filename : register_filenames)
    {
        std::cout << "filename: " << filename << std::endl;

        int pos = pattern.size() - 3;
        int len = filename.size() - pos - 4; // .jpg|.bmp|.png
        std::string username = filename.substr(pos, len);
        std::cout << "username: " << username << std::endl;

        cv::Mat img = cv::imread(filename, 1);
        double cf=640.0/img.cols;
        double rf=640.0/img.rows;
        if(cf<rf)
        {
            cf=rf;
        }
        cv::resize(img, img, cv::Size(int(img.cols*cf), int(img.rows*cf)));
        std::vector<FaceObject> faceobjects = detector->detect(img);

        if(faceobjects.size()<1)
        {
            std::cout << "error : " << filename <<"  detect face failed"<< std::endl;
            continue;

        }
        int i=0;
        int ind=0;
        double minfdist=img.cols*img.cols*img.rows*img.rows;

        for(const auto &obj : faceobjects)
        {
            double fdx=(img.cols/2-(obj.rect.x+obj.rect.width/2));
            double fdy=(img.rows/2-(obj.rect.y+obj.rect.height/2));
            double fdd=fdx*fdx+fdy*fdy;
            if(minfdist>fdd)
            {
                minfdist=fdd;
                ind=i;
            }
            i++;
        }
        if(faceobjects[ind].rect.width<50||faceobjects[ind].rect.height<50)
        {
            std::cout << "error : " << filename <<"  detect face too small"<< std::endl;
            continue;

        }

        cv::Mat aligned_face;
        std::vector<cv::Point2f> landmark = faceobjects[ind].landmark;
        std::vector<float> feature;
        cv::Mat Im2=img.clone();
        recognizer->crop_face(img, landmark, aligned_face);
        //cv::rectangle(Im2, cv::Rect(cv::Point(faceobjects[ind].rect.x, faceobjects[ind].rect.y), cv::Size(faceobjects[ind].rect.width, faceobjects[ind].rect.height)),
        //          cv::Scalar(255, 255, 0), 2);
        //cv::circle(Im2,landmark[0],2, cv::Scalar(0, 255, 0),2,8,0);
        //cv::circle(Im2,landmark[1],2, cv::Scalar(0, 255, 0),2,8,0);
        //cv::circle(Im2,landmark[2],2, cv::Scalar(0, 255, 0),2,8,0);
        //cv::circle(Im2,landmark[3],2, cv::Scalar(0, 255, 0),2,8,0);
        //cv::circle(Im2,landmark[4],2, cv::Scalar(0, 255, 0),2,8,0);
        //cv::imshow("test2", Im2(cv::Rect(faceobjects[ind].rect)));
        //cv::imwrite("/home/fz/wj-work/face/EasyFace-main/images/test2.jpg", Im2);
        //cv::imshow("test1", aligned_face);
        //cv::waitKey(2000);
        recognizer->extract_feature(aligned_face, feature);

        username_feature_map.insert(std::make_pair(username, feature));
    }

    std::cout << "Finish face register. " << std::endl;
}
double FaceProcessor::get_dist(cv::Point2f p1,cv::Point2f p2)
{
    double dist=sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
    return dist;

}
double FaceProcessor::get_front_pose_ratio(const std::vector<cv::Point2f> &landmark )
{
    double r_a=get_dist(landmark[0],landmark[2]);
    double r_b=get_dist(landmark[2],landmark[3]);
    double r_c=get_dist(landmark[0],landmark[3]);
    double r_p=(r_a+r_b+r_c)/2.0;
    double r_area=sqrt(r_p*(r_p-r_a)*(r_p-r_b)*(r_p-r_c));

    double l_a=get_dist(landmark[1],landmark[2]);
    double l_b=get_dist(landmark[2],landmark[4]);
    double l_c=get_dist(landmark[1],landmark[4]);
    double l_p=(l_a+l_b+l_c)/2.0;
    double l_area=sqrt(l_p*(l_p-l_a)*(l_p-l_b)*(l_p-l_c));

    double area_ratio=r_area/(l_area+0.000001);
    if (r_area>l_area)
    {area_ratio=l_area/(r_area+0.000001);}

    //
    double v1_x=landmark[1].x-landmark[0].x;
    double v1_y=landmark[1].y-landmark[0].y;
    double v2_x=landmark[1].x-landmark[0].x;
    double v2_y=landmark[0].y-landmark[0].y;

    double hori_ratio=(v1_x*v2_x+v1_y*v2_y)/(sqrt(v1_x*v1_x+v1_y*v1_y)*sqrt(v1_x*v1_x+v1_y*v1_y));

    return (area_ratio+hori_ratio)/2;

}

bool FaceProcessor::prepro_detect_result(const FaceObject &obj,cv::Mat &img,int &flag)
{

    // face roi size

    bool is_small=(obj.rect.height<30||obj.rect.width<30);
    if(is_small)
    {
        flag=1;
        return false;
    }
    //score
    if(obj.prob<0.9)
    {
        flag=2;
       return false;
    }

    //in im center
    int invpad=10;
    cv::Rect RecIn(invpad,invpad,img.cols-2*invpad,img.rows-2*invpad);
    cv::Rect rec_obj(obj.rect);
    bool is_in=(rec_obj==(RecIn&rec_obj));
    std::cout<<" is_in "<<is_in<<std::endl;
    if(!is_in)
    {
        flag=3;
        return false;
    }

    //get front face
    double pose_ratio=get_front_pose_ratio(obj.landmark );
    std::cout<<" pose_ratio "<<pose_ratio<<std::endl;
    if(pose_ratio<0.85)
    {
        flag=4;
        return false;
    }

    cv::Mat face_im=img(rec_obj).clone();
    cv::Mat face_gray;
    cv::cvtColor(face_im,face_gray,CV_RGB2GRAY);
    cv::Mat out_im;
    cv::Laplacian(face_gray,out_im,CV_16S,3);
    cv::Mat tmp_m,tmp_sd;

    cv::meanStdDev(out_im,tmp_m,tmp_sd);
    double sd=tmp_sd.at<double>(0,0);
    std::cout<<" sd "<<sd<<std::endl;

    if (sd<15)
    {
        flag=5;
        return false;
    }
    flag=0;
    return true;

}
std::string FaceProcessor::get_max_similarity(std::vector<float> &feature,const std::map<std::string,std::vector<float>> &username_feature_map)
{
    int lensmooth=2;

    std::string name;
    float max_similarity = -FLT_MAX;
    for(const auto& item : username_feature_map)
    {
        std::string username = item.first;
        std::vector<float> register_feature = item.second;
        float similarity = recognizer->calculate_similarity(feature, register_feature);
        if(similarity > max_similarity)
        {
            max_similarity = similarity;
            name = username;
        }
    }
    float threshold = 0.55;
    if(max_similarity < threshold)
    {
        name = "unknown";
    }
    int num=username_num_map.count(name);
    if(num>0)
    {
       if(username_num_map[name]>lensmooth)
       {
           name=name;
       }
       else
       {
           username_num_map[name]=username_num_map[name]+1;
           name="NAN";
       }
    }
    else
    {
        username_num_map.clear();
        username_num_map[name]=1;
        name="NAN";
    }

    return name;

}

void FaceProcessor::face_identify(easyface::FaceDetector *detector,
                                  easyface::FaceRecognizer *recognizer,
                                  const std::map<std::string,std::vector<float>> &username_feature_map,
                                  cv::Mat &img,cv::Rect &face_roi,std::string &name_result,int &iflag)
{
    std::cout << "Start face identify " << std::endl;



    if(img.empty())
    {
        std::cout << "blank frame grabbed\n";
        return;
    }

    // identify
    cv::GaussianBlur(img,img,cv::Size(3,3),0.2);
    std::vector<FaceObject> faceobjects = detector->detect(img);

    double fontscale = 1;
    int thickness = 2;
    int baseLine = 2;

    for(const auto &obj : faceobjects)
    {
        //>>>>>>>>>>>>>>>>>>>>>>>>>>>>

        //<<<<<<<<<<<<<<<<<<<<<<<<<<<

        int flag=0;

        if(!prepro_detect_result(obj,img,flag))
        {
            //>>>>>>>>>>>>>>>>>>>>>>>>>>>>
            std::string result;
            if(flag==3)
            {
                result="keep face in center of camera";
            }
            else if(flag==4)
            {
                result="not frontal face  ";
            }
            else if(flag==5)
            {
                result=" image too blur  ";
            }
            iflag=flag;
            face_roi=cv::Rect(obj.rect);
            char text[256];
            sprintf(text, "%s", result.c_str());
            printf("detect result : %s\n", text);
            cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, fontscale, thickness, &baseLine);

            int x = obj.rect.x;
            int y = obj.rect.y - label_size.height - baseLine;
            if (y < 0)
                y = 0;
            if (x + label_size.width > img.cols)
                x = img.cols - label_size.width;



            cv::rectangle(img, cv::Rect(cv::Point(obj.rect.x, obj.rect.y), cv::Size(obj.rect.width, obj.rect.height)),
                      cv::Scalar(255, 255, 0), 1);

            cv::rectangle(img, cv::Rect(cv::Point(x, y), cv::Size(label_size.width, label_size.height + baseLine)),
                      cv::Scalar(255, 255, 255), -1);

            cv::putText(img, text, cv::Point(x, y + label_size.height),
                    cv::FONT_HERSHEY_SIMPLEX, fontscale, cv::Scalar(0, 255, 0), thickness);

            //<<<<<<<<<<<<<<<<<<<<<<<<<<<
            continue;
        }
        face_roi=cv::Rect(obj.rect);
        iflag=flag;
        cv::Mat aligned_face;
        std::vector<cv::Point2f> landmark = obj.landmark;
        std::vector<float> feature;
        recognizer->crop_face(img, landmark, aligned_face);
        recognizer->extract_feature(aligned_face, feature);

        std::string match_result=get_max_similarity(feature,username_feature_map);
        if(match_result=="NAN")
        {

          continue;
        }
        name_result=match_result;
        //>>>>>>>>>>>>>>>>>>>>>>>>>>>>
        char text[256];
        sprintf(text, "%s", match_result.c_str());
        printf("identify result: %s\n", text);
        cv::Size label_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, fontscale, thickness, &baseLine);

        int x = obj.rect.x;
        int y = obj.rect.y - label_size.height - baseLine;
        if (y < 0)
            y = 0;
        if (x + label_size.width > img.cols)
            x = img.cols - label_size.width;

        cv::rectangle(img, cv::Rect(cv::Point(obj.rect.x, obj.rect.y), cv::Size(obj.rect.width, obj.rect.height)),
                  cv::Scalar(0, 255, 0), 4);

        cv::rectangle(img, cv::Rect(cv::Point(x, y), cv::Size(label_size.width, label_size.height + baseLine)),
                  cv::Scalar(255, 255, 255), -1);

        cv::putText(img, text, cv::Point(x, y + label_size.height),
                cv::FONT_HERSHEY_SIMPLEX, fontscale, cv::Scalar(0, 255, 0), thickness);
        //<<<<<<<<<<<<<<<<<<<<<<<<<<<

    }

    //std::string out_filename = identify_filename.substr(0, str_len - 4) + ".result.jpg";
    //printf("save result to: %s\n", out_filename.c_str());
    //cv::imwrite(out_filename, img);
    // show live and wait for a key with timeout long enough to show images

//    imshow("face identify", img);
//    char key = cv::waitKey(1);
//    if (key == 27)
//        break;



    std::cout << "Finish face identify. " << std::endl;
}


void FaceProcessor::setRegisterImagepath(QString path) {
    this->register_imagepath = path.toStdString();
}


void FaceProcessor::regis() {


    face_register(detector, recognizer, register_imagepath, username_feature_map);
    return;

}
void FaceProcessor::identify(cv::Mat &frame,cv::Rect &face_roi,std::string &result,int &flag) {

    face_identify(detector, recognizer, username_feature_map,frame,face_roi,result,flag);
    return;

}
